
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Aircraft.h
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <common/gp_math/gp_mathlib.h>
#include <common/location/location.h>

#include "Battery.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    Vector3f_t    gyro;           //角速度(rad/s)
    Quat_t        quat;           //姿态四元数（起点为牵连坐标系）
    Euler_t        euler;

    Vector3f_t    acc_body;
    Vector3f_t    velocity_ef;
    Vector3f_t    velocity_bf;
    Vector3f_t    position;
}FlyState;

typedef struct
{
    FlyState state;
    Location home;
    bool home_is_set;
    Location location;
    Vector3f_t mag_bf;

    float groundlevel;    //地面高度
    float home_yaw;
    float frame_height; //飞行器高度
}AirCraft;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void Aircraft_SetStartLoc(AirCraft* pAirCraft, const Location start_loc, const float start_yaw);
float AirCraft_GetAltAboveGround(AirCraft* pAirCraft);
void AirCraft_UpdateLoc(AirCraft* pAirCraft);
void AirCraft_UpdateDynamics(FlyState* pState, Vector3f_t* rot_acc, Vector3f_t* body_acc, float dt);
FlyState* AirCraft_GetState(AirCraft* pAircraft);
Location AirCraft_GetLocation(AirCraft* pAircraft);
void AirCraft_Publish(AirCraft* pAircraft);
Quat_t* AirCraft_GetQuat(AirCraft* pAircraft);
Quat_t AirCraft_GetQuat_q(AirCraft* pAircraft);
float AirCraft_GetRefAlt(AirCraft* pAircraft);

void AirCraft_UpdateLoc_v1(AirCraft* pAirCraft, float dt);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif

